#include <Def.h>
#include <cmath>
#include <qdebug.h>
#include "Algo.h"
#include "GeodeSys.h"

// These are hard numbers from the WGS84 standard.  DON'T MODIFY
// unless you want to change the datum.
#define _EQURAD 6378137.0
#define _FLATTENING 298.257223563

// These are derived quantities more useful to the code:
#if 0
#define _SQUASH (1 - 1/_FLATTENING)
#define _STRETCH (1/_SQUASH)
#define _POLRAD (EQURAD * _SQUASH)
#else
// High-precision versions of the above produced with an arbitrary
// precision calculator (the compiler might lose a few bits in the FPU
// operations).  These are specified to 81 bits of mantissa, which is
// higher than any FPU known to me:
#define _SQUASH    0.9966471893352525192801545
#define _STRETCH   1.0033640898209764189003079
#define _POLRAD    6356752.3142451794975639668
#endif

// The constants from the WGS84 standard
const double GeodeSys::EQURAD = _EQURAD;
const double GeodeSys::iFLATTENING = _FLATTENING;
const double GeodeSys::SQUASH = _SQUASH;
const double GeodeSys::STRETCH = _STRETCH;
const double GeodeSys::POLRAD = _POLRAD;

// additional derived and precomputable ones
// for the geodetic conversion algorithm

#define E2 fabs(1 - _SQUASH*_SQUASH)
static double a = _EQURAD;
static double ra2 = 1 / (_EQURAD*_EQURAD);
//static double e = sqrt(E2);
static double e2 = E2;
static double e4 = E2 * E2;

#undef _EQURAD
#undef _FLATTENING
#undef _SQUASH
#undef _STRETCH
#undef _POLRAD
#undef E2

double GeodeSys::GeodPointToSeaLevelRadius(const GeodPoint& geod)
{
	// this is just a simplified version of the GeodPointToCart function above,
	// substitute h = 0, take the 2-norm of the cartesian vector and simplify
	double phi = geod.getLatitudeRad();
	double sphi = sin(phi);
	double sphi2 = sphi * sphi;
	return a * sqrt((1 + (e4 - 2 * e2)*sphi2) / (1 - e2 * sphi2));
}

// Notes:
//
// The XYZ/cartesian coordinate system in use puts the X axis through
// zero lat/lon (off west Africa), the Z axis through the north pole,
// and the Y axis through 90 degrees longitude (in the Indian Ocean).
//
// All latitude and longitude values are in radians.  Altitude is in
// meters, with zero on the WGS84 ellipsoid.
//
// The code below makes use of the notion of "squashed" space.  This
// is a 2D cylindrical coordinate system where the radius from the Z
// axis is multiplied by SQUASH; the earth in this space is a perfect
// circle with a radius of POLRAD.

////////////////////////////////////////////////////////////////////////
//
// Direct and inverse distance functions 
//
// Proceedings of the 7th International Symposium on Geodetic
// Computations, 1985
//
// "The Nested Coefficient Method for Accurate Solutions of Direct and
// Inverse Geodetic Problems With Any Length"
//
// Zhang Xue-Lian
// pp 747-763
//
// modified for FlightGear to use WGS84 only -- Norman Vine

static inline double M0(double e2) 
{
	//double e4 = e2*e2;
	return Algo::pi()*0.5*(1.0 - e2 * (1.0 / 4.0 + e2 * (3.0 / 64.0 +
		e2 * (5.0 / 256.0))));
}

// given, lat1, lon1, az1 and distance (s), calculate lat2, lon2
// and az2.  Lat, lon, and azimuth are in degrees.  distance in meters
static int _geo_direct_wgs_84(double lat1,double lon1,double az1,
	double s,double *lat2,double *lon2,double *az2)
{
	double a = GeodeSys::EQURAD,rf = GeodeSys::iFLATTENING;
	double testv = 1.0E-10;
	double f = (rf > 0.0 ? 1.0 / rf : 0.0);
	double b = a * (1.0 - f);
	double e2 = f * (2.0 - f);
	double phi1 = Algo::deg2rad(lat1),lam1 = Algo::deg2rad(lon1);
	double sinphi1 = sin(phi1),cosphi1 = cos(phi1);
	double azm1 = Algo::deg2rad(az1);
	double sinaz1 = sin(azm1),cosaz1 = cos(azm1);

	if(fabs(s) < 0.01) 
	{	// distance < centimeter => congruency
		*lat2 = lat1;
		*lon2 = lon1;
		*az2 = 180.0 + az1;
		if (*az2 > 360.0) *az2 -= 360.0;
		return 0;
	}
	else if(std::numeric_limits<double>::min() < fabs(cosphi1)) 
	{	
		// non-polar origin
        // u1 is reduced latitude
		double tanu1 = sqrt(1.0 - e2)*sinphi1 / cosphi1;
		double sig1 = atan2(tanu1,cosaz1);
		double cosu1 = 1.0 / sqrt(1.0 + tanu1 * tanu1),sinu1 = tanu1 * cosu1;
		double sinaz = cosu1 * sinaz1,cos2saz = 1.0 - sinaz * sinaz;
		double us = cos2saz * e2 / (1.0 - e2);

		// Terms
		double	ta = 1.0 + us * (4096.0 + us * (-768.0 + us * (320.0 - 175.0*us))) / 16384.0,
			tb = us * (256.0 + us * (-128.0 + us * (74.0 - 47.0*us))) / 1024.0,
			tc = 0;

		// FIRST ESTIMATE OF SIGMA (SIG)
		double first = s / (b*ta);  // !!
		double sig = first;
		double c2sigm,sinsig,cossig,temp,denom,rnumer,dlams,dlam;
		do {
			c2sigm = cos(2.0*sig1 + sig);
			sinsig = sin(sig); cossig = cos(sig);
			temp = sig;
			sig = first +
				tb * sinsig*(c2sigm + tb * (cossig*(-1.0 + 2.0*c2sigm*c2sigm) -
					tb * c2sigm*(-3.0 + 4.0*sinsig*sinsig)
					*(-3.0 + 4.0*c2sigm*c2sigm) / 6.0)
					/ 4.0);
		} while (fabs(sig - temp) > testv);

		// LATITUDE OF POINT 2
		// DENOMINATOR IN 2 PARTS (TEMP ALSO USED LATER)
		temp = sinu1 * sinsig - cosu1 * cossig*cosaz1;
		denom = (1.0 - f)*sqrt(sinaz*sinaz + temp * temp);

		// NUMERATOR
		rnumer = sinu1 * cossig + cosu1 * sinsig*cosaz1;
		*lat2 = Algo::rad2deg(atan2(rnumer,denom));

		// DIFFERENCE IN LONGITUDE ON AUXILARY SPHERE (DLAMS )
		rnumer = sinsig * sinaz1;
		denom = cosu1 * cossig - sinu1 * sinsig*cosaz1;
		dlams = atan2(rnumer,denom);

		// TERM C
		tc = f * cos2saz*(4.0 + f * (4.0 - 3.0*cos2saz)) / 16.0;

		// DIFFERENCE IN LONGITUDE
		dlam = dlams - (1.0 - tc)*f*sinaz*(sig + tc * sinsig*
			(c2sigm +
				tc * cossig*(-1.0 + 2.0*
					c2sigm*c2sigm)));
		*lon2 = Algo::rad2deg(lam1 + dlam);
		if (*lon2 > 180.0) *lon2 -= 360.0;
		if (*lon2 < -180.0) *lon2 += 360.0;

		// AZIMUTH - FROM NORTH
		*az2 = Algo::rad2deg(atan2(-sinaz,temp));
		if (fabs(*az2) < testv) *az2 = 0.0;
		if (*az2 < 0.0) *az2 += 360.0;
		return 0;
	}
	else 
	{			
		// phi1 == 90 degrees, polar origin
		double dM = a * M0(e2) - s;
		double paz = (phi1 < 0.0 ? 180.0 : 0.0);
		double zero = 0.0f;
		return _geo_direct_wgs_84(zero,lon1,paz,dM,lat2,lon2,az2);
	}
}

bool GeodeSys::direct(const GeodPoint& p1,double course1,double distance,GeodPoint& p2,double& course2)
{
	double lat2,lon2;
	int ret = _geo_direct_wgs_84(p1.getLatitudeDeg(),p1.getLongitudeDeg(),
		course1,distance,&lat2,&lon2,&course2);
	p2.setLatitudeDeg(lat2);
	p2.setLongitudeDeg(lon2);
	p2.setElevationM(0);
	return ret == 0;
}

GeodPoint GeodeSys::direct(const GeodPoint& p1,double course1,double distance,bool* ok)
{
	GeodPoint p2;
	double lat2,lon2,course2;
	int ret = _geo_direct_wgs_84(p1.getLatitudeDeg(),p1.getLongitudeDeg(),
		course1,distance,&lat2,&lon2,&course2);
	if(ret != 0) 
	{
		if(ok)
			*ok = false;

		qDebug() << "call _geo_direct_wgs_84 failed";
		printGeoPoint(p1);
		qDebug()<< "course1 = " << course1 << ",distance = " << distance;;
		return p2;
	}

	if(ok)
		*ok = true;

	p2.setLatitudeDeg(lat2);
	p2.setLongitudeDeg(lon2);
	p2.setElevationM(0);
	return p2;
}

// given lat1, lon1, lat2, lon2, calculate starting and ending
// az1, az2 and distance (s).  Lat, lon, and azimuth are in degrees.
// distance in meters
static int _geo_inverse_wgs_84(double lat1,double lon1,double lat2,double lon2,double *az1,double *az2,double *s)
{
	double a = GeodeSys::EQURAD,rf = GeodeSys::iFLATTENING;
	int iter = 0;
	double testv = 1.0E-10;
	double f = (rf > 0.0 ? 1.0 / rf : 0.0);
	double b = a * (1.0 - f);
	// double e2 = f*(2.0-f); // unused in this routine
	double phi1 = Algo::deg2rad(lat1),lam1 = Algo::deg2rad(lon1);
	double sinphi1 = sin(phi1),cosphi1 = cos(phi1);
	double phi2 = Algo::deg2rad(lat2),lam2 = Algo::deg2rad(lon2);
	double sinphi2 = sin(phi2),cosphi2 = cos(phi2);

	if((fabs(lat1 - lat2) < testv &&
		(fabs(lon1 - lon2) < testv)) || (fabs(lat1 - 90.0) < testv))
	{
		// TWO STATIONS ARE IDENTICAL : SET DISTANCE & AZIMUTHS TO ZERO */
		*az1 = 0.0; *az2 = 0.0; *s = 0.0;
		return 0;
	}
	else if(fabs(cosphi1) < testv) 
	{
		// initial point is polar
		int k = _geo_inverse_wgs_84(lat2,lon2,lat1,lon1,az1,az2,s);
		Q_UNUSED(k);

		b = *az1; 
		*az1 = *az2; 
		*az2 = b;
		return 0;
	}
	else if(fabs(cosphi2) < testv) 
	{
		// terminal point is polar
		double _lon1 = lon1 + 180.0f;
		int k = _geo_inverse_wgs_84(lat1,lon1,lat1,_lon1,az1,az2,s);
		Q_UNUSED(k);

		*s /= 2.0;
		*az2 = *az1 + 180.0;
		if(*az2 > 360.0) 
			*az2 -= 360.0;
		return 0;
	}
	else if((fabs(fabs(lon1 - lon2) - 180) < testv) && (fabs(lat1 + lat2) < testv))
	{
		// Geodesic passes through the pole (antipodal)
		double s1,s2;
		_geo_inverse_wgs_84(lat1,lon1,lat1,lon2,az1,az2,&s1);
		_geo_inverse_wgs_84(lat2,lon2,lat1,lon2,az1,az2,&s2);
		*az2 = *az1;
		*s = s1 + s2;
		return 0;
	}
	else 
	{
		// antipodal and polar points don't get here
		double dlam = lam2 - lam1,dlams = dlam;
		double sdlams,cdlams,sig,sinsig,cossig,sinaz,cos2saz,c2sigm;
		double tc,temp,us,rnumer,denom,ta,tb;
		double cosu1,sinu1,sinu2,cosu2;

		// Reduced latitudes
		temp = (1.0 - f)*sinphi1 / cosphi1;
		cosu1 = 1.0 / sqrt(1.0 + temp * temp);
		sinu1 = temp * cosu1;
		temp = (1.0 - f)*sinphi2 / cosphi2;
		cosu2 = 1.0 / sqrt(1.0 + temp * temp);
		sinu2 = temp * cosu2;

		do 
		{
			sdlams = sin(dlams),cdlams = cos(dlams);
			sinsig = sqrt(cosu2*cosu2*sdlams*sdlams +
				(cosu1*sinu2 - sinu1 * cosu2*cdlams)*
				(cosu1*sinu2 - sinu1 * cosu2*cdlams));
			cossig = sinu1 * sinu2 + cosu1 * cosu2*cdlams;

			sig = atan2(sinsig,cossig);
			sinaz = cosu1 * cosu2*sdlams / sinsig;
			cos2saz = 1.0 - sinaz * sinaz;
			c2sigm = (sinu1 == 0.0 || sinu2 == 0.0 ? cossig :
				cossig - 2.0*sinu1*sinu2 / cos2saz);
			tc = f * cos2saz*(4.0 + f * (4.0 - 3.0*cos2saz)) / 16.0;
			temp = dlams;
			dlams = dlam + (1.0 - tc)*f*sinaz*
				(sig + tc * sinsig*
				(c2sigm + tc * cossig*(-1.0 + 2.0*c2sigm*c2sigm)));
			if(fabs(dlams) > Algo::pi() && iter++ > 50) 
			{
				return iter;
			}
		} while(fabs(temp - dlams) > testv);

		us = cos2saz * (a*a - b * b) / (b*b); // !!
		// BACK AZIMUTH FROM NORTH
		rnumer = -(cosu1*sdlams);
		denom = sinu1 * cosu2 - cosu1 * sinu2*cdlams;
		*az2 = Algo::rad2deg(atan2(rnumer,denom));
		if(fabs(*az2) < testv) 
			*az2 = 0.0;
		if(*az2 < 0.0) 
			*az2 += 360.0;

		// FORWARD AZIMUTH FROM NORTH
		rnumer = cosu2 * sdlams;
		denom = cosu1 * sinu2 - sinu1 * cosu2*cdlams;
		*az1 = Algo::rad2deg(atan2(rnumer,denom));
		if(fabs(*az1) < testv) 
			*az1 = 0.0;
		if(*az1 < 0.0) 
			*az1 += 360.0;

		// Terms a & b
		ta = 1.0 + us * (4096.0 + us * (-768.0 + us * (320.0 - 175.0*us))) /
			16384.0;
		tb = us * (256.0 + us * (-128.0 + us * (74.0 - 47.0*us))) / 1024.0;

		// GEODETIC DISTANCE
		*s = b * ta*(sig - tb * sinsig*
			(c2sigm + tb * (cossig*(-1.0 + 2.0*c2sigm*c2sigm) - tb *
				c2sigm*(-3.0 + 4.0*sinsig*sinsig)*
				(-3.0 + 4.0*c2sigm*c2sigm) / 6.0) /
				4.0));
		return 0;
	}
}

bool GeodeSys::inverse(const GeodPoint& p1,const GeodPoint& p2,double& course1,double& course2,double& distance)
{
	int ret = _geo_inverse_wgs_84(p1.getLatitudeDeg(),p1.getLongitudeDeg(),
		p2.getLatitudeDeg(),p2.getLongitudeDeg(),
		&course1,&course2,&distance);
	return ret == 0;
}

double GeodeSys::courseDeg(const GeodPoint& p1,const GeodPoint& p2,bool* ok)
{
	double course1,course2,distance;
	int r = _geo_inverse_wgs_84(p1.getLatitudeDeg(),p1.getLongitudeDeg(),
		p2.getLatitudeDeg(),p2.getLongitudeDeg(),
		&course1,&course2,&distance);
	
	if(r != 0) 
	{
		if(ok)
			*ok = false;
		qDebug()<<"GeodeSys::courseDeg, unable to compute course";
		printGeoPoint(p1);
		printGeoPoint(p2);
	}

	if (ok)
		*ok = true;
	return course1;
}

double GeodeSys::distanceM(const GeodPoint& p1,const GeodPoint& p2,bool* ok)
{
	double course1,course2,distance = 0.0;
	int r = _geo_inverse_wgs_84(p1.getLatitudeDeg(),p1.getLongitudeDeg(),
		p2.getLatitudeDeg(),p2.getLongitudeDeg(),
		&course1,&course2,&distance);
	if(r != 0) 
	{
		if(ok)
			*ok = false;
		
		qDebug()<< "GeodeSys::distanceM, unable to compute distance";
		printGeoPoint(p1);
		printGeoPoint(p2);
	}

	if(ok)
		*ok = true;
	return distance;
}

double GeodeSys::distanceNm(const GeodPoint& from,const GeodPoint& to)
{
	return distanceM(from,to) * SDK_METER_TO_NM;
}
 